# Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os

ROS_RAW_TYPES = [
    "bool",
    "byte",
    "char",
    "duration",
    "float32",
    "float64",
    "int16",
    "int32",
    "int64",
    "int8",
    "string",
    "time",
    "uint16",
    "uint32",
    "uint64",
    "uint8",
]
ROS_INSTALL_PATH = "/opt/ros/noetic"


def ExpandMsg(fn, _node):
    with open(fn, "r") as f:
        ls = f.readlines()
    ret = []
    for l in ls:
        msg_ele = DefMsg()
        l = l.strip()
        if "#" in l:
            l = l.split("#")[0].strip()
        if not l:
            continue
        if "=" in l:  # 常量
            msg_ele.isConst = True
            l, msg_ele.constValue = l.split("=")
            l = l.strip()
        while "  " in l:  # 把所有两个空格变成1个空格
            l = l.replace("  ", " ")
        try:
            _type, _name = l.split(" ")
            if _type.endswith("]"):  # 数组
                msg_ele.isArray = True
                _type, _size = _type.split("[")
                _size = _size[:-1]
                if _size.isdigit():  # 固定长度数组
                    msg_ele.arraySize = int(_size)
                _type = _type.strip()

            if _type in ROS_RAW_TYPES:
                _nodeName = "raw"
            elif "/" in _type:
                _nodeName, _type = _type.split("/")
                fn = _nodeName + "/msg/" + _type + ".msg"
                if not os.path.exists(os.path.join(ROS_INSTALL_PATH, "share", fn)):
                    print("!!! msg file not found1:", fn)
                    exit()
            else:
                _nodeName = _node
                fn = _nodeName + "/msg/" + _type + ".msg"
                if not os.path.exists(os.path.join(ROS_INSTALL_PATH, "share", fn)):
                    _nodeName = "std_msgs"
                    fn = (
                        os.path.join(ROS_INSTALL_PATH, "share/std_msgs/msg", _type)
                        + ".msg"
                    )
                    if not os.path.exists(fn):
                        print("!!! msg file not found2:[%s]" % fn, _node)
                        exit()
            msg_ele.node = _nodeName
            msg_ele.type = _type
            msg_ele.name = _name
            ret.append(msg_ele)
        except:
            print("err:[", l, "]", l.split(" "))
            exit()
    return ret


class DefMsg:
    def __init__(self):
        self.isConst = False
        self.constValue = ""
        self.isArray = False
        self.arraySize = 0
        # self.node = ""
        # self.type = ""
        # self.name = ""


def put_value_by_type(pt, d, value):
    # print("    ",d.type,d.isArray)
    if d.isArray:
        if d.type == "bool":
            pt.put_bool_array(value)
        elif d.type == "byte":
            pt.put_byte_array(value)
        elif d.type == "char":
            pt.put_char_array(value)
        elif d.type == "duration":
            pt.put_duration_array(value)
        elif d.type == "float32":
            pt.put_float32_array(value)
        elif d.type == "float64":
            pt.put_float64_array(value)
        elif d.type == "int16":
            pt.put_int16_array(value)
        elif d.type == "int32":
            pt.put_int32_array(value)
        elif d.type == "int64":
            pt.put_int64_array(value)
        elif d.type == "int8":
            pt.put_int8_array(value)
        elif d.type == "string":
            pt.put_string_array(value)
        elif d.type == "time":
            pt.put_time_array(value)
        elif d.type == "uint16":
            pt.put_uint16_array(value)
        elif d.type == "uint32":
            pt.put_uint32_array(value)
        elif d.type == "uint64":
            pt.put_uint64_array(value)
        elif d.type == "uint8":
            pt.put_uint8_array(value)
        else:
            msg_type = d.node + "/" + d.type
            msgObj = EnvManager.gi().GetMsgObj(msg_type)
            # print("array len:", len(value))
            pt.put_uint32(len(value))
            for i in range(len(value)):
                msgObj.put_msg(pt, value[i])
    else:
        if d.type == "bool":
            pt.put_bool(value)
        elif d.type == "byte":
            pt.put_byte(value)
        elif d.type == "char":
            pt.put_char(value)
        elif d.type == "duration":
            pt.put_duration(value)
        elif d.type == "float32":
            pt.put_float32(value)
        elif d.type == "float64":
            pt.put_float64(value)
        elif d.type == "int16":
            pt.put_int16(value)
        elif d.type == "int32":
            pt.put_int32(value)
        elif d.type == "int64":
            pt.put_int64(value)
        elif d.type == "int8":
            pt.put_int8(value)
        elif d.type == "string":
            pt.put_string(value)
        elif d.type == "time":
            pt.put_time(value)
        elif d.type == "uint16":
            pt.put_uint16(value)
        elif d.type == "uint32":
            pt.put_uint32(value)
        elif d.type == "uint64":
            pt.put_uint64(value)
        elif d.type == "uint8":
            pt.put_uint8(value)
        else:
            msg_type = d.node + "/" + d.type
            EnvManager.gi().GetMsgObj(msg_type).put_msg(pt, value)


def get_value_by_type(pt, d, msg, prefix):
    if d.isArray:
        if d.type == "bool":
            exec("%s = pt.get_bool_array()" % prefix, locals())
        elif d.type == "byte":
            exec("%s = pt.get_byte_array()" % prefix, locals())
        elif d.type == "char":
            exec("%s = pt.get_char_array()" % prefix, locals())
        elif d.type == "duration":
            exec("%s = pt.get_duration_array()" % prefix, locals())
        elif d.type == "float32":
            exec("%s = pt.get_float32_array()" % prefix, locals())
        elif d.type == "float64":
            exec("%s = pt.get_float64_array()" % prefix, locals())
        elif d.type == "int16":
            exec("%s = pt.get_int16_array()" % prefix, locals())
        elif d.type == "int32":
            exec("%s = pt.get_int32_array()" % prefix, locals())
        elif d.type == "int64":
            exec("%s = pt.get_int64_array()" % prefix, locals())
        elif d.type == "int8":
            exec("%s = pt.get_int8_array()" % prefix, locals())
        elif d.type == "string":
            exec("%s = pt.get_string_array()" % prefix, locals())
        elif d.type == "time":# todo
            exec("%s = pt.get_time_array()" % prefix, locals())
        elif d.type == "uint16":
            exec("%s = pt.get_uint16_array()" % prefix, locals())
        elif d.type == "uint32":
            exec("%s = pt.get_uint32_array()" % prefix, locals())
        elif d.type == "uint64":
            exec("%s = pt.get_uint64_array()" % prefix, locals())
        elif d.type == "uint8":
            exec("%s = pt.get_uint8_array()" % prefix, locals())
        else:
            exec("from %s.msg import %s"%(d.node,d.type),locals())
            msg_type = d.node + "/" + d.type
            msgObj = EnvManager.gi().GetMsgObj(msg_type)
            count = pt.get_uint32()
            for i in range(count):
                exec("%s.append(%s())"%(prefix,d.type),locals())
                msgObj.get_msg(pt, msg, prefix + "[%d]" % i)
            # print("-----",count,msg)
    else:
        if d.type == "bool":
            exec("%s = pt.get_bool()" % prefix, locals())
        elif d.type == "byte":
            exec("%s = pt.get_byte()" % prefix, locals())
        elif d.type == "char":
            exec("%s = pt.get_char()" % prefix, locals())
        elif d.type == "duration":
            exec("%s = pt.get_duration()" % prefix, locals())
        elif d.type == "float32":
            exec("%s = pt.get_float32()" % prefix, locals())
        elif d.type == "float64":
            exec('''%s = pt.get_float64()'''%prefix, locals())
        elif d.type == "int16":
            exec("%s = pt.get_int16()" % prefix, locals())
        elif d.type == "int32":
            exec("%s = pt.get_int32()" % prefix, locals())
        elif d.type == "int64":
            exec("%s = pt.get_int64()" % prefix, locals())
        elif d.type == "int8":
            exec("%s = pt.get_int8()" % prefix, locals())
        elif d.type == "string":
            exec("%s = pt.get_string()" % prefix, locals())
        elif d.type == "time":
            # exec("%s = pt.get_time()" % prefix, locals())
            t = pt.get_time()
            exec("%s.secs = %d" % (prefix,t["secs"]), locals())
            exec("%s.nsecs = %d" % (prefix,t["nsecs"]), locals())
        elif d.type == "uint16":
            exec("%s = pt.get_uint16()" % prefix, locals())
        elif d.type == "uint32":
            exec("%s = pt.get_uint32()" % prefix, locals())
        elif d.type == "uint64":
            exec("%s = pt.get_uint64()" % prefix, locals())
        elif d.type == "uint8":
            exec("%s = pt.get_uint8()" % prefix, locals())
        else:
            msg_type = d.node + "/" + d.type
            msgObj = EnvManager.gi().GetMsgObj(msg_type)
            msgObj.get_msg(pt, msg, prefix)


class BaseMsg:
    def __init__(self, fn, nn):
        _, name = os.path.split(fn)
        self.nodeName = nn
        self.path = fn
        self.name = os.path.splitext(name)[0]
        self.defs = ExpandMsg(fn, nn)

    def show(self):
        print("nodeName:", self.nodeName)
        print("path:", self.path)
        print("name:", self.name)
        print("defs:")
        print("node\ttype\tname\tisConst\tconstValue\tisArray\tarraySize")
        for d in self.defs:
            print(
                d.node, d.type, d.name, d.isConst, d.constValue, d.isArray, d.arraySize
            )

    def put_struct(self, pt):
        pt.put_uint32(len(self.defs))
        for d in self.defs:
            pt.put_string(d.node)
            pt.put_string(d.type)
            pt.put_string(d.name)
            pt.put_bool(d.isConst)
            pt.put_string(d.constValue)
            pt.put_bool(d.isArray)
            pt.put_uint32(d.arraySize)

    def put_msg(self, pt, msg):
        # print(self.name)
        for d in self.defs:
            # print("    ", d.type, d.name, d.isConst)
            if d.isConst:
                # todo 写入常量值
                pass
            else:
                put_value_by_type(pt, d, eval("msg.%s" % d.name))

    def get_msg(self, pt, msg, prefix="msg"):
        for d in self.defs:
            if d.isConst:
                # todo 读取常量值
                pass
            else:
                get_value_by_type(pt, d, msg, prefix + "." + d.name)
        return msg


class NodeMsg:
    def __init__(self, name):
        self.name = name
        self.msgs = {}


class EnvManager:
    instance = None

    @staticmethod
    def gi():
        if EnvManager.instance is None:
            EnvManager.instance = EnvManager()
        return EnvManager.instance

    def __init__(self):
        self.nodes = {}
        self.Scan()

    def Scan(self):
        pth = os.path.join(ROS_INSTALL_PATH, "share")
        for nodeName in os.listdir(pth):
            nodePath = os.path.join(pth, nodeName)
            if os.path.isdir(nodePath):
                msgPath = os.path.join(nodePath, "msg")
                if os.path.exists(msgPath) and os.path.isdir(msgPath):
                    for msgName in os.listdir(msgPath):
                        if msgName.endswith(".msg"):
                            fn = os.path.join(msgPath, msgName)
                            if nodeName not in self.nodes:
                                self.nodes[nodeName] = NodeMsg(nodeName)
                            self.nodes[nodeName].msgs[msgName[:-4]] = BaseMsg(
                                fn, nodeName
                            )

    def GetMsgObj(self, msg):
        node, name = msg.split("/")
        if node not in self.nodes:
            return None
        if name not in self.nodes[node].msgs:
            for n in self.nodes[node].msgs:
                print(n, name)
            return None
        return self.nodes[node].msgs[name]


if __name__ == "__main__":
    EnvManager.gi()

    msg = EnvManager.gi().GetMsgObj("sensor_msgs/Image")
    msg.show()
